#pragma once

struct float4x4;
float4 mul(const float4& v,const float4x4& m);
struct float4x4
{
	union
	{
		struct
		{
			float _11,_12,_13,_14;
			float _21,_22,_23,_24;
			float _31,_32,_33,_34;
			float _41,_42,_43,_44;
		};
		struct
		{
			float m[16];
		};
	};

	float4& operator[](int index)
	{
		return (float4&)m[4*index];
	}
	const float4& operator[](int index) const
	{
		return (float4&)m[4*index];
	}


	float3& axis(int index)
	{
		return (float3&)(*this)[index];
	}

	const float3& axis(int index) const
	{
		return (float3&)(*this)[index];
	}

	float3& translate()
	{
		return (float3&)(*this)[3];
	}
	const float3& translate() const
	{
		return (float3&)(*this)[3];
	}
	void identity()
	{
		_11=1; _12=0; _13=0; _14=0;
		_21=0; _22=1; _23=0; _24=0;
		_31=0; _32=0; _33=1; _34=0;
		_41=0; _42=0; _43=0; _44=1;
	}

	void transpose()
	{
		swap(_12,_21); swap(_13,_31);swap(_14,_41);
		swap(_23,_32);swap(_24,_42);
		swap(_34,_43);
	}

	void from_rotation(float yaw, float pitch, float roll)
	{
		float sx=(sinf(pitch)), cx=(cosf(pitch));
		float sy=(sinf(yaw)), cy=(cosf(yaw));
		float sz=(sinf(roll)), cz=(cosf(roll));

		_11 = cz*cy-sz*sx*sy;	_12 = sz*cx;	_13 = -sz*sx*cy-cz*sy;
		_21 = -cz*sx*sy-sz*cy;	_22 = cz*cx;	_23 = sz*sy-cz*sx*cy;
		_31 = cx*sy;			_32 = sx;		_33 = cx*cy;
	}

	void from_rotation_xyz(float yaw, float pitch, float roll)
	{
	}

	void to_rotation(float& yaw, float& pitch, float& roll) const
	{

		if (fabsf(_32)<0.9999f)
		{
			roll=atan2f(_12, _22);
			yaw=atan2f(_31, _33);
			pitch= asinf(_32);
		}
		else
		{
			roll=0.0f;
			yaw=atan2f(-sign(_32)*_21, _11);
			pitch=PI/2*sign(_32);
		}
	}

	void compose(const float3& i_scale, const quat& i_rot, const float3& i_trans)
	{
		float wx,wy,wz,xx,xy,xz,yy,yz,zz;
		float3 Two;
		quat NormQuat;

		NormQuat=i_rot;//Normalize(q); // ha nem normalizalt, erdekes lesz... de most remeljuk, hogy az

		Two=float3(NormQuat.x+NormQuat.x,NormQuat.y+NormQuat.y,NormQuat.z+NormQuat.z);

		wx=NormQuat.w*Two.x;wy=NormQuat.w*Two.y;wz=NormQuat.w*Two.z;
		xx=NormQuat.x*Two.x;xy=NormQuat.x*Two.y;xz=NormQuat.x*Two.z;
		yy=NormQuat.y*Two.y;yz=NormQuat.y*Two.z;zz=NormQuat.z*Two.z;

		(*this)[0]=float4((1.f-(yy+zz)),	(xy+wz),        (xz-wy), 0)*i_scale.x;
		(*this)[1]=float4((xy-wz),        (1.f-(xx+zz)),   (yz+wx), 0)*i_scale.y;
		(*this)[2]=float4((xz+wy),        (yz-wx),        (1.f-(xx+yy)), 0)*i_scale.z;
		(*this)[3]=float4(i_trans.x, i_trans.y, i_trans.z, 1.f);
	}



	float3 get_scale() const
	{
		return float3(length(axis(0)),length(axis(1)),length(axis(2)));
	}

	void scale(float s)
	{
		axis(0)*=s;
		axis(1)*=s;
		axis(2)*=s;
	}

	void scale(float x,float y, float z)
	{
		axis(0)*=x;
		axis(1)*=y;
		axis(2)*=z;
	}
	void mul(const float4x4& m1,const float4x4& m2)
	{
		(*this)[0]=::mul(m1[0],m2);
		(*this)[1]=::mul(m1[1],m2);
		(*this)[2]=::mul(m1[2],m2);
		(*this)[3]=::mul(m1[3],m2);
	}


	void invert(const float4x4& i_src)
	{
		float fA0 = i_src.m[ 0]*i_src.m[ 5] - i_src.m[ 1]*i_src.m[ 4];
		float fA1 = i_src.m[ 0]*i_src.m[ 6] - i_src.m[ 2]*i_src.m[ 4];
		float fA2 = i_src.m[ 0]*i_src.m[ 7] - i_src.m[ 3]*i_src.m[ 4];
		float fA3 = i_src.m[ 1]*i_src.m[ 6] - i_src.m[ 2]*i_src.m[ 5];
		float fA4 = i_src.m[ 1]*i_src.m[ 7] - i_src.m[ 3]*i_src.m[ 5];
		float fA5 = i_src.m[ 2]*i_src.m[ 7] - i_src.m[ 3]*i_src.m[ 6];
		float fB0 = i_src.m[ 8]*i_src.m[13] - i_src.m[ 9]*i_src.m[12];
		float fB1 = i_src.m[ 8]*i_src.m[14] - i_src.m[10]*i_src.m[12];
		float fB2 = i_src.m[ 8]*i_src.m[15] - i_src.m[11]*i_src.m[12];
		float fB3 = i_src.m[ 9]*i_src.m[14] - i_src.m[10]*i_src.m[13];
		float fB4 = i_src.m[ 9]*i_src.m[15] - i_src.m[11]*i_src.m[13];
		float fB5 = i_src.m[10]*i_src.m[15] - i_src.m[11]*i_src.m[14];

		float fDet = fA0*fB5-fA1*fB4+fA2*fB3+fA3*fB2-fA4*fB1+fA5*fB0;
		m[ 0] =+ i_src.m[ 5]*fB5 - i_src.m[ 6]*fB4 + i_src.m[ 7]*fB3;
		m[ 4] =- i_src.m[ 4]*fB5 + i_src.m[ 6]*fB2 - i_src.m[ 7]*fB1;
		m[ 8] =+ i_src.m[ 4]*fB4 - i_src.m[ 5]*fB2 + i_src.m[ 7]*fB0;
		m[12] =- i_src.m[ 4]*fB3 + i_src.m[ 5]*fB1 - i_src.m[ 6]*fB0;
		m[ 1] =- i_src.m[ 1]*fB5 + i_src.m[ 2]*fB4 - i_src.m[ 3]*fB3;
		m[ 5] =+ i_src.m[ 0]*fB5 - i_src.m[ 2]*fB2 + i_src.m[ 3]*fB1;
		m[ 9] =- i_src.m[ 0]*fB4 + i_src.m[ 1]*fB2 - i_src.m[ 3]*fB0;
		m[13] =+ i_src.m[ 0]*fB3 - i_src.m[ 1]*fB1 + i_src.m[ 2]*fB0;
		m[ 2] =+ i_src.m[13]*fA5 - i_src.m[14]*fA4 + i_src.m[15]*fA3;
		m[ 6] =- i_src.m[12]*fA5 + i_src.m[14]*fA2 - i_src.m[15]*fA1;
		m[10] =+ i_src.m[12]*fA4 - i_src.m[13]*fA2 + i_src.m[15]*fA0;
		m[14] =- i_src.m[12]*fA3 + i_src.m[13]*fA1 - i_src.m[14]*fA0;
		m[ 3] =- i_src.m[ 9]*fA5 + i_src.m[10]*fA4 - i_src.m[11]*fA3;
		m[ 7] =+ i_src.m[ 8]*fA5 - i_src.m[10]*fA2 + i_src.m[11]*fA1;
		m[11] =- i_src.m[ 8]*fA4 + i_src.m[ 9]*fA2 - i_src.m[11]*fA0;
		m[15] =+ i_src.m[ 8]*fA3 - i_src.m[ 9]*fA1 + i_src.m[10]*fA0;

		float fInvDet = 1.0f/fDet;
		m[ 0] *= fInvDet;
		m[ 1] *= fInvDet;
		m[ 2] *= fInvDet;
		m[ 3] *= fInvDet;
		m[ 4] *= fInvDet;
		m[ 5] *= fInvDet;
		m[ 6] *= fInvDet;
		m[ 7] *= fInvDet;
		m[ 8] *= fInvDet;
		m[ 9] *= fInvDet;
		m[10] *= fInvDet;
		m[11] *= fInvDet;
		m[12] *= fInvDet;
		m[13] *= fInvDet;
		m[14] *= fInvDet;
		m[15] *= fInvDet;
	}

	void normalize()
	{
		::normalize(axis(2));
		axis(0).cross(axis(1),axis(2));
		::normalize(axis(0));
		axis(1).cross(axis(2),axis(0));
	}

	void normalize_yzx()
	{
		::normalize(axis(1));
		axis(0).cross(axis(1),axis(2));
		::normalize(axis(0));
		axis(2).cross(axis(0),axis(1));
	}


	void rotate(const float3& i_axis, float i_angle)
	{
		float cosangle=cosf(i_angle);
		float sinangle=sinf(i_angle);
		this->axis(0)=::rotate(this->axis(0),i_axis,sinangle,cosangle);
		this->axis(1)=::rotate(this->axis(1),i_axis,sinangle,cosangle);
		this->axis(2)=::rotate(this->axis(2),i_axis,sinangle,cosangle);
		//		normalize();
	}
	static const float4x4 identity_matrix;

};

__forceinline float4 mul(const float4& v, const float4x4& m)
{
	return float4(
		m._11*v.x+m._21*v.y+m._31*v.z+m._41*v.w,
		m._12*v.x+m._22*v.y+m._32*v.z+m._42*v.w,
		m._13*v.x+m._23*v.y+m._33*v.z+m._43*v.w,
		m._14*v.x+m._24*v.y+m._34*v.z+m._44*v.w
		);

}

__forceinline float3 mul4x3(const float3& v,const float4x4& m)
{
	return float3(
		m._11*v.x+m._21*v.y+m._31*v.z+m._41,
		m._12*v.x+m._22*v.y+m._32*v.z+m._42,
		m._13*v.x+m._23*v.y+m._33*v.z+m._43
		);
}

__forceinline float3 mul3x3(const float3& v,const float4x4& m)
{
	return float3(
		m._11*v.x+m._21*v.y+m._31*v.z,
		m._12*v.x+m._22*v.y+m._32*v.z,
		m._13*v.x+m._23*v.y+m._33*v.z
		);
}

__forceinline float4x4 get_projection_matrix(float fov,float aspect,float nearz, float farz)
{
	float tanhalffov=tanf(fov/2);

	float4x4 projection_matrix;
	float h, w, Q;

	w = 1/(tanhalffov*aspect);
	h = 1/tanhalffov;
	Q = farz/(farz-nearz);

	projection_matrix._11 = w; projection_matrix._12=0; projection_matrix._13=0; projection_matrix._14=0;
	projection_matrix._21=0; projection_matrix._22 = h; projection_matrix._23=0; projection_matrix._24=0;
	projection_matrix._31=0; projection_matrix._32=0; projection_matrix._33 = Q; projection_matrix._34=1;
	projection_matrix._41=0; projection_matrix._42=0; projection_matrix._43 = -Q*nearz; projection_matrix._44=0;

	return projection_matrix;
}

__forceinline float4x4 get_projection_matrix_tanhalffov(float tanhalffov,float aspect,float nearz, float farz)
{
	float4x4 projection_matrix;
	float h, w, Q;

	w = 1/(tanhalffov*aspect);
	h = 1/tanhalffov;
	Q = farz/(farz-nearz);

	projection_matrix._11 = w; projection_matrix._12=0; projection_matrix._13=0; projection_matrix._14=0;
	projection_matrix._21=0; projection_matrix._22 = h; projection_matrix._23=0; projection_matrix._24=0;
	projection_matrix._31=0; projection_matrix._32=0; projection_matrix._33 = Q; projection_matrix._34=1;
	projection_matrix._41=0; projection_matrix._42=0; projection_matrix._43 = -Q*nearz; projection_matrix._44=0;

	return projection_matrix;
}

